#include <stdio.h>
#include <stdlib.h>
#include <windows.h>
#include <iostream>
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>

using namespace std;

#include "cv.h"
#include "cxcore.h"
#include "cvaux.h"
#include "ml.h"
#include "highgui.h"
#include "number.h"
#include "imageloader.h"

static int x1 = 0, x2 = 0, x3 = 0, x4= 0, x5 = 0;
float a = 0, b = 0, c =0;
static int sizeh = 500;
static int sizew = 500;
float _angle = 0, _angleT = 0,  _angleT2 = 0, pos =0;; 
int i, j;
int num=8;
double len = 10;
float  count=0;


float body[4][3] = {{2,1,5},{2,1,6},{2,2,4}, {4,4,8}};
float bodyOffset[3][3] = {{0, 1, 0},{0, 1, -6},{0, 1, 6}};
float bodyOffset1[2][3] = {{0, 1, -4},{0, 1, 8}};

void TestReadBuffer(int h, int w)
{
	int thesize = sizeof(char) * w * h * 3;
	char* pixels = (char*)malloc( thesize );
	glReadPixels( 0, 0, w, h, GL_RGB, GL_UNSIGNED_BYTE, pixels );
	IplImage* img = cvCreateImage( cvSize(w,h), IPL_DEPTH_8U, 3 );
	cvZero(img);
	int step       = img->widthStep/sizeof(uchar);
	int channels   = img->nChannels;
	uchar* data    = (uchar *)img->imageData;
	
	for( int k = 0; k < thesize; k+=3 ) {
		int i = h - k / 3 / h - 1; // something is wrong here, be more smart
		int j = (k / 3) % h;
		int r = (int) pixels[k];
		int g = (int) pixels[k+1];
		int b = (int) pixels[k+2];
		data[i*step+j*channels+0]=b; // B
		data[i*step+j*channels+1]=g; // G
		data[i*step+j*channels+2]=r; // R
		int breakhere = 1;
	}
	cvSaveImage( "TestRobotArmDraw.png", img );
	cvReleaseImage( &img );
	free( pixels );
	int breakhere = 1;
}



//Makes the image into a texture, and returns the id of the texture
GLuint loadTexture(Image* image) {
	GLuint textureId;
	glGenTextures(1, &textureId); //Make room for our texture
	glBindTexture(GL_TEXTURE_2D, textureId); //Tell OpenGL which texture to edit
	//Map the image to the texture
	glTexImage2D(GL_TEXTURE_2D,                //Always GL_TEXTURE_2D
				 0,                            //0 for now
				 GL_RGB,                       //Format OpenGL uses for image
				 image->width, image->height,  //Width and height
				 0,                            //The border of the image
				 GL_RGB, //GL_RGB, because pixels are stored in RGB format
				 GL_UNSIGNED_BYTE, //GL_UNSIGNED_BYTE, because pixels are stored
				                   //as unsigned numbers
				 image->pixels);               //The actual pixel data
	return textureId; //Returns the id of the texture
}

GLuint _textureId; //The id of the texture



void initRendering(void)
{
	glEnable(GL_DEPTH_TEST);
	glEnable(GL_LIGHTING);
	glEnable(GL_LIGHT0);
	glEnable(GL_LIGHT1);
	glEnable(GL_NORMALIZE);
	glEnable(GL_COLOR_MATERIAL);
	glShadeModel(GL_SMOOTH);
	//glClearColor(1,1 ,.7,1);

	Image* image = loadBMP("test.bmp");
	_textureId = loadTexture(image);
	delete image;

}

void light()
{

	//Ambient Light
	float ambientColor[]= {0.5f, 0.5f, 0.5f, 1 };
	glLightModelfv(GL_LIGHT_MODEL_AMBIENT, ambientColor);
	
	//Positioned Light 1
	float lightColor0[] ={1.0f, 1.0f, 1.0f};
	float lightPos0[] = {2.0f, 2.0f, 10.0f, 1.0f};
	glLightfv(GL_LIGHT0, GL_DIFFUSE, lightColor0);
	glLightfv(GL_LIGHT0, GL_POSITION, lightPos0);

	//Directed Light 2
	float lightColor1[] = {1.0f, 1.0f, 1.0f};
	float lightPos1[] = {-2.0f, -2.0f, 10.0f, 1.0f};
	glLightfv(GL_LIGHT1, GL_DIFFUSE, lightColor1);
	glLightfv(GL_LIGHT1, GL_POSITION, lightPos1);
	

}

void update (int value)
{
count= count + 1;
_angleT = 15*sin((3.14159265/180)*count);
_angleT2 = -5*cos((3.14159265/180)*count);
pos = pos+1.09/180;
if (_angle > 360) {
	_angle -= 360;
}
if (_angleT > 360) {
	_angleT -= 360;
}
if (_angleT2 > 360) {
	_angleT2 -= 360;
}
if (pos >6.2)
{
	pos=-6.2;
}
glutPostRedisplay();
glutTimerFunc(25 , update, 0);

}





void drawLimb(float scaleF, float trans)
{
	//limb	
	glPushMatrix();
	glTranslatef(scaleF/2, 0.0f, 0.0f);	
	glScalef(scaleF, 1 , 1 );
	//glColor3f(0.0f, 0.0f, 0.5f);
	glutSolidSphere(.5,100,100);
	glPopMatrix();
}
void drawJoint(int scale)
{	
	glPushMatrix();
	//glColor3f(0.5f,0,0);
	glScalef(scale,scale,scale);
	glutSolidSphere(.5,100,100);
	glPopMatrix();
}
void arm(float a, float b, float c,  double x, double y, double z, int i, int gh)
{

	//float length = 0,leq = 0;
	//length=30*cos(45);
	//leq=2*sin(-(180/(3.14159265))*atan2(z , x))*length*sin(_angle2);


	glPushMatrix();
	glTranslatef(x,y,z);
	glRotatef(-(180/(3.14159265))*atan2(z , x)-(_angleT*pow((-1),double (i%2))), 0.0f, 1.0f , 0.0f);
	glRotatef(-_angleT2*pow((-1),double (gh%2)), 0.0f, 0.0f , 1.0f);
	glPushMatrix();
	//first Limb	
	glRotatef(a, 0.0f, 0.0f , 1.0f);	
	drawLimb(5, 0);
	drawJoint(1);

	//second Limb	
	glTranslatef(5,0,0);
	glRotatef(b, 0.0f, 0.0f , 1.0f);	
	drawLimb(10, 0);
	drawJoint(1);

	//third Limb
	glTranslatef(10,0,0);
	glRotatef(c, 0, 0, 1);
	drawLimb(15, 0);
	drawJoint(1);
	glPopMatrix();
	glPopMatrix();
}

void legLocation (double N, double M) {
	a=-45;
	b=90;
	c=-90;	
	double inc;
	double *loc = new double[(num/2)*3 ];

	inc=len/(num/2+1);
	for (i=0 ; i < num/2 ; i++ )
	{	
		loc[3*i]=sqrt(N*N*(1-(len/2-(inc*(i+1)))*(len/2-(inc*(i+1)))/M/M));
		loc[3*i+1]=0;
		loc[3*i+2]=-(len/2-(inc*(i+1)));

	}	
	
	for( i=0 ; i < num/2; i++ )
		{	
			arm(a,b,c,loc[3*i]*pow((-1),double (i%2)),loc[3*i+1],loc[3*i+2],i, 1);
		}
	for( i=0 ; i < num/2; i++ )
		{	
			arm(a,b,c,-loc[3*i]*pow((-1),double (i%2)),-loc[3*i+1],loc[3*i+2],i, 0);
		}
	delete loc;
}
void drawBody (void)
{
	//MidBody
	glPushMatrix();
	glScalef(body[0][0],body[0][1],body[0][2]);
	glColor3f(.4,.4,.4);
	glutSolidSphere(1,100,100);
	glPopMatrix();

	glPushMatrix();
	glTranslatef(bodyOffset[0][0],bodyOffset[0][1],bodyOffset[0][2]);	
	glScalef(body[1][0],body[1][1]+1,body[1][2]);
	//glColor3f(.1,.1,.1);
	glutSolidSphere(1,100,100);
	glPopMatrix();


	//head
	glPushMatrix();
	glTranslatef(bodyOffset[1][0],bodyOffset[1][1],bodyOffset[1][2]);	
	drawJoint(1);
	glRotatef(-15, 1.0f, 0.0f , 0.0f);
	glTranslatef(bodyOffset1[0][0],bodyOffset1[0][1],bodyOffset1[0][2]);
	glScalef(body[2][0],body[2][1],body[2][2]);
	//glColor3f(.1,.1,.1);
	glutSolidSphere(1,100,100);
	glPopMatrix();

	//thorax
	glPushMatrix();	
	glTranslatef(bodyOffset[2][0],bodyOffset[2][1],bodyOffset[2][2]);
	drawJoint(1);
	glRotatef(-15, 1.0f, 0.0f , 0.0f);
	glTranslatef(bodyOffset1[1][0],bodyOffset1[1][1],bodyOffset1[1][2]);
	glScalef(body[3][0],body[3][1],body[3][2]);
	//glColor3f(.1,.1,.1);
	glutSolidSphere(1,100,100);
	glPopMatrix();
}
void background(void)
{
	glPushMatrix();
	glScalef(10,10,10);
	glEnable(GL_TEXTURE_2D);
	glBindTexture(GL_TEXTURE_2D, _textureId);
	
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
	//glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
	glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
	glBegin(GL_QUADS);
	glColor3f(.5,.5,.5);
	


	glNormal3f(0.0, 0.0f, 1.0f);
	glTexCoord2f(0.0f, 0.0f);
	glVertex3f(-1.0f, -1.0f, 0);
	glTexCoord2f(.5f, 0.0f);
	glVertex3f(1.0f, -1.0f, 0);
	glTexCoord2f(.50f, .50f);
	glVertex3f(1.0f, 1.0f,0);
	glTexCoord2f(0.0f, 0.50f);
	glVertex3f(-1.0f, 1.0f, 0);
	
	glEnd();

glPopMatrix();

}
void drawBug(void)
{
	float d = 18.437;
	//necessary to beging the production of the structures
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glTranslatef(0.0f ,0.0f ,-10.0f);
	glRotatef(110, 6.0f, 2.0f , 12.0f);
	glRotatef(75, 0.0f, 1.0f , 0.0f);
	background();
	glTranslatef(0.0f ,pos ,0);
	glTranslatef(0.0f ,0.0f ,-2.236*sin((3.14159265/180)*(-d-abs(_angleT2))));
	glRotatef(90, 1.0f, 0.0f , 0.0f);
	//glRotatef(90, 1.0f, 0.0f , 0.0f);
	//glRotatef(_angle, 1.0f, 0.0f , 0.0f);
	glScalef(.1,.1,.1); 
	drawBody();
	legLocation(body[0][0],body[0][2]);



	

	//Places the shapes from the 3d space to the 2d window
	glutSwapBuffers();
}

void reshape( int w, int h)
{
	sizeh = h;
	sizew = w;

	glViewport(0, 0, (GLsizei) w, (GLsizei) h);
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	gluPerspective(45.0, (GLfloat) w/(GLfloat) h, 1.0, 200.0);
}


void keyboard( unsigned char key, int x, int y )
{
	switch (key) {
		case 'a':
			x1 = (x1+5 )% 360;
			glutPostRedisplay();
			break;
		case 'A':
			x1 = (x1 - 5) % 360;
			glutPostRedisplay();
			break;
		case 's': 
			x2 = (x2 + 5) % 360;
			glutPostRedisplay();
			break;
		case 'S':
			x2 = (x2 - 5) % 360;
			glutPostRedisplay();
			break;
		case 'd': 
			x3 = (x3 + 5) % 360;
			glutPostRedisplay();
			break;
		case 'D':
			x3 = (x3 - 5) % 360;
			glutPostRedisplay();
			break;
		case 'f': 
			x4 = (x4 + 5) % 360;
			glutPostRedisplay();
			break;
		case 'F':
			x4 = (x4 - 5) % 360;
			glutPostRedisplay();
			break;
		case 'g': 
			x5 = (x5 + 5) % 360;
			glutPostRedisplay();
			break;
		case 'G':
			x5 = (x5 - 5) % 360;
			glutPostRedisplay();
			break;
		case 't':
			cout<<"Saving current frame buffer to image! \n ";
			TestReadBuffer( sizeh, sizew );
			break;
		case 27:
			exit(0);
		default:
			break;
	}
}

int main( int argc, char** argv )
{
	//Required Inits
	glutInit(&argc, argv);
	glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
	glutInitWindowSize(sizeh,sizew);
	glutInitWindowPosition(100,100);
	glutCreateWindow( argv[0]);
	
	//Getting the Drawing Going
	initRendering();
	glutDisplayFunc(drawBug);
	glutReshapeFunc(reshape);
	glutKeyboardFunc(keyboard);
	glutTimerFunc(25, update, 0);	
	//The Show
	glutMainLoop();
	return 0;
}